#!/usr/bin/env python
import roslib; roslib.load_manifest('Eyebox2')
import sys
import rospy
from std_msgs.msg import String
from pr2_controllers_msgs.msg import *
from Eyebox2.msg import *
from geometry_msgs.msg import *
from actionlib import *
from datetime import time


class heads_down():
    def __init__(self):
        self.eyebox_message_sub = rospy.Subscriber("/eyebox_message",eye_box_messages,self.callback)
        self.PointHeadClient = SimpleActionClient("/head_traj_controller/point_head_action",PointHeadAction)
        self.PointHeadClient.wait_for_server()
        print 'done initializing'

    def callback(self,data):
        if True in data.eyes_looking:
#            print 'Look Down'
            goal = PointHeadGoal()
            goal.target.header.frame_id= 'base_link'
            goal.target.point.x = 1
            goal.target.point.y = 0
            goal.target.point.z = 0
            
            '''goal.target = point
            goal.pointing_frame = "wide_stereo_optical_frame"
            goal.pointing_axis.x = 1
            goal.pointing_axis.y = 0
            goal.pointing_axis.z = 0'''

            self.PointHeadClient.send_goal(goal)
#            self.PointHeadClient.wait_for_result(rospy.rostime.Duration.from_sec(1))
        else:
#            print "looking up"
            goal = PointHeadGoal()
            goal.target.header.frame_id= 'base_link'
            goal.target.point.x = 1.2
            goal.target.point.y = 0
            goal.target.point.z = 1.2
            
            '''goal.target = point
            goal.pointing_frame = "wide_stereo_optical_frame"
            goal.pointing_axis.x = 1
            goal.pointing_axis.y = 0
            goal.pointing_axis.z = 0'''

            self.PointHeadClient.send_goal(goal)
#            self.PointHeadClient.wait_for_result(rospy.rostime.Duration.from_sec(1))


if __name__ == '__main__':
    rospy.init_node('heads_down', anonymous=True)
    headManager = heads_down()

    try:
      rospy.spin()
    except KeyboardInterrupt:
      print "Shutting down"

